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1.  INTRODUCTION 


The  main  objective  of  this  research  is  to  develop  the  tracking  algorithms  for  a 
multi-sensor  tracking  mount  control  system.  The  tracking  mount  responds  to 
independent  drive  signals  about  the  azimuth  and  elevation  axes.  The  tracking 
error  is  detected  by  means  of  four  independent  sensors,  namely,  RF  telemetry 
system,  millimeter  wave  radar,  infrared  sensor,  and  TV  video  system.  The  quality 
of  the  signal  from  each  of  the  sensor  systems  is  available  in  terms  of  measures 
of  the  signal  to  noise  ratio.  It  is  assumed  that  these  in  turn  can  be  expressed 
as  the  variance  of  the  statistical  measurement  noise  distribution. 

A  major  requirement  of  the  tracking  mount  control  system  is  the  development  of 
tracking  filters  to  reduce  the  noise  content  of  the  multi-sensor  output 
information.  Fixed  gain  tracking  filter  algorithms,  such  as  the  a-B  and  a~B“Y 
tracking  filters  have  been  used  in  this  application  in  the  past  in  conjunction 
with  single  sensor  measurements.  This  study  focuses  on  two  aspects.  First  the 
U3e  of  Kalman  filters  is  considered  for  the  tracking  application.  Secondly,  the 
techniques  for  the  fusion  of  multi-sensory  information  are  presented.  A 
functional  block  diagram  representation  of  the  multi-sensory  tracking  mount 
control  system  is  shown  in  Figure  1. 

The  general  formulation  of  the  Kalman  filter  is  presented  first.  The  precise 
form  of  the  algorithm  would  depend  on  the  model  that  is  selected  for  the  time 
evolution  of  the  quantity  being  measured.  Two  such  cases  are  considered  in 
detail.  In  the  first,  the  target  acceleration  is  modeled  as  white  noise  and  a 
two-state  Kalman  filter  is  developed.  Next,  the  possibility  of  a  correlated 
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target  acceleration  is  allowed  for  by  modeling  the  target  acceleration  as  a 
Gauss-Markov  process.  This  results  in  an  algorithm  involving  a  three-state 
Kalman  filter.  In  each  case,  the  possibility  of  adapting  the  filter  in  the 
presence  of  target  maneuvers  is  discussed.  This  is  followed  by  a  presentation  of 
two  approaches  for  fusion  of  the  multi-sensor  information.  Recommended  processing 
hardware  and  estimated  processing  times  are  presented. 


A  computer  simulation  program  for  the  evaluation  of  the  tracking  filter 
performance  is  described  next.  Results  of  a  single  run  using  the  two-state  and 
the  three-state  Kalman  filters  are  presented. 
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2.  GENERAL  FORMULATION  OF  THE  DISCRETE  KALMAN  FILTER 


The  general  formulation  of  the  discrete  Kalman  filter  is  briefly  presented  in 
this  section. 


Let  the  random  process  to  be  estimated  be  described  by  the  linear  discrete  state 
equation 

x(k+l )  =  $(k)x(k)  +  u(k)  +  w(k)  _ (1) 

The  measurement  of  the  process  is  assumed  to  occur  in  accordance  with  the  linear 
relationship 

z(k)=  H(k)x(k)  +  v(k)  _ (2) 

Here,  the  various  quantities  are  defined  as: 

x(k)  =  nxl  process  state  vector  at  time  t^ 

<$>(k)  =  nxn  process  state  transition  matrix 
u(k)  =  nxl  deterministic  input  vector  at  time  t^ 
w(k)  =  nxl  input  noise  vector  assumed  to  be  a  white  sequence 
with  known  covariance 
z(k)  =  mxl  measurement  vector  at  time  t^ 

H(k)  =  mxn  measurement  matrix  describing  the  ideal 

(noiseless)  connection  between  the  measurement  and 
state  vectors  at  time  t^ 

v(k)  =  mxl  measurement  noise  vector  assumed  to  be  a  white 
sequence  with  known  covariance 
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The  covariance  matrices  for  w(k)  and  v(k)  are  given  by, 


E[w(k)wT(i)] 
E[ v(k)vT( i ) ] 


(QOO 

,  i=k 

io 

,  i/*k 

R(k) 

,  i=k 

0 

,  i  A 

TM-3628 


(3) 


The  input  and  measurement  noise  vectors  are  assumed  to  be  uncorreiated, 

E[w(k)v^(i)]  =  0  ,  for  all  k  and  i.  _ (4) 

Let  us  assume  that  at  some  point  in  time  t^,  an  initial  estimate  x~(k)  is 
available  based  on  all  of  our  knowledge  about  the  process  prior  to  the  arrival  of 
the  measurement  z(k). 


This  is  known  as  the  a  priori  estimate.  The  "hat"  denotes  the  estimate  and  the 
super  minus  indicates  it's  a  priori  nature.  The  estimation  error  then  becomes 

e— (k)  =  x(k)  -  x— (k)  (5) 

We  further  assume  that  the  associated  error  covariance  matrix 

T  T 

P  (k)  =  E[e  (k)e  (k> ]  =  E[(x(k)-x  (k))(x(k)-x  (k))  ] 

_ (6) 

is  also  known. 

With  the  arrival  of  the  measurement  z(k)  at  time  t^,  let  the  estimate  be  updated 
according  to  the  relation 

x(k)  =x“(k)  +  K(k) (z(k)-H(k)x“(k) )  (7) 

where  x(k)  represents  the  updated  (or  a  posteriori)  estimate  and  K(k)  denotes  a 
nxm  gain  matrix. 
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The  updated  estimation  error  and  its  covariance  matrix  are; 
e(k)  =  x(k)  -  x(k) 


(8) 


T  T 

P(k)  =  E[e(k)e  (k)]  =  E[(x(k)-x(k))(x(k)-x(k))  ] 

_ (9) 

Subsitution  of  Eqn.  (2)  into  (7),  and  the  resulting  expression  for  x(k)  into 
Eqn.  (9)  leads  to: 

T  T 

P(k)  =(I-K(k)H(k))P“(k)(I-K(k)H(k))  +  K(k)R(k)K  (k) 

_ (10) 

The  individual  elements  along  the  major  diagonal  of  P(k)  represent  the  estimation 
error  variance  of  the  elements  of  the  state  vector  which  are  being  estimated. 

The  Kalman  filter  selects  the  gain  matrix  K(k)  such  that  each  element  on  the 
major  diagonal  of  P(k)  is  minimized.  The  optimum  gain  K(k),  known  as  the  Kalman 
gain,  is  given  by: 

T  T  -1 

K(k)  =  P~(k)H  (k)(H(k)P“(k)H  (k)  +  R(k))  (11) 

The  covariance  matrix  P(k),  using  the  Kalman  gain,  can  be  shown  to  be  given  by 

P(k)  =(I-K(k)H(k) )P— (k)  (12) 

For  the  next  time  point  t^+1*  the  a  priori  estimate  is  obtained  from  Eqn.  (1) 

as, 

x— (k+1)  =  $(k)x(k)  +  u(k)  (13) 

in  view  of  the  fact  that  w(k)  is  a  white  sequence  (with  zero  mean  and  time-wise 
uncorrelated) . 

The  a  priori  error  and  associated  covariance  at  time  t^+i  are  then  obtained  as, 

e~ (k+1)  =  x(k+l)  -  x— (k+1)  =  4>(k)e(k)  +  w(k)  _ (14) 

T  T 

P"(k+1)  =  E[e— (k+1  )e—  (k+1)]  =  *(k)P(k)$  (k)  +  Q(k) 

_ (15) 


The  Kalman  filter  algorithm  may  be  summarized  as  follows: 
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1.  Obtain  a  priori  estimate  x  (k)  and  its  error  covariance  P  (k) 

2.  Compute  Kalman  gain 


T  T  -1 

K(k)  =  P"(k)H  (k)(H(k)P~(k)H  (k)  +  R(k))  _ (16) 

3.  Update  estimate  with  measurement  z(k) 

x(k)  =x— (k)  +  K(k)(z(k)-H(k)x— (k) )  _ (17) 

4.  Compute  error  covariance  for  updated  estimate 

P(k)  =(I-K(k)H(k))P~(k)  _ (18) 

5.  Project  ahead 

x~(k+l)  =  $(k)x(k)  +  u(k)  _ (19) 

T 

P_(k+1)  =  $(k)P(k)4>  (k)  +  Q(k)  _ (20) 

6.  Set  k  =  k+1  and  go  to  step  2. 


A  schematic  diagram  of  the  Kalman  filter  loop  is  shown  in  Figure  2. 
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3.  TWO-STATE  KALMAN  FILTER 
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3.1  Filter  Algorithm.  The  explicit  form  of  the  Kalman  filter  for  the  tracking 

mount  azimuth  or  elevation  estimation  using  a  two-state  system  model  is  obtained 

in  this  section.  To  this  end,  define  the  state  variables: 

xi  =  9C  -  9m  ,  tracking  error 
o 

x2  =  9t  ,  target  velocity 

The  continuous  state  equations  then  are 
o 

xi  =  X2  -  <%(t) 
o 

X2  =  n(t)  _ (21) 

o 

where  <am(t)  denotes  the  tracking  mount  rate.  The  target  acceleration  X2 
is  modeled  as  a  zero  mean  white  process  n(t)  with  auto  correlation  function 


2 

o  6(t ) . 
a 


The  discrete  model  corresponding  to  the  continuous  state  equations  may  be  derived 
as 


(22) 


x i ( k+ 1 )  =  xi(k)  +  Tx2(k)  +  A9m(k)  +  wj(k) 

X2(k+1)  =  X2(k)  +  W2 (k) 

where  A9m(k)  =  9m(k+l)  -  9m(k)  represents  the  change  in  the  mount  encoder  reading 
during  the  sample  period  T,  and  is  treated  as  a  deterministic  input  in  this 
application.  The  covariance  of  the  random  sequence  w(k)  is  obtained  as, 


--  - 

*1  , 

= 

qn 

qi2 

II 

Q 

TJ/3 

TV2 

qi2 

q22 

a 

TV2  T  | 

_  _J 

Q(k)  = 

The  measurement  z  is  given  by 

z(k)  =  xi(k)  +  v(k) 


with  R(k)=  o  ,  the  variance  of  the  sensor  error, 
v 


(23) 


(24) 
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The  matrices  characterizing  the  model  are  thus: 


4>(k)  = 


T" 

1_ 


H(k)  =  [1  0] 


(25) 


Letting 


P“(k)  = 


Pll 

P12 


P(k)  = 


Pll 


P12 


P12 

P22 


(26) 


and  subsituting  the  <J>(k),  H(k)  expressions  into  the  general  Kalman  filter 
equations  of  chapter  2,  the  algorithm  for  the  two  state  model  is  found  to  be, 

1.  Obtain  a  priori  estimates  x]“ (k),  X2  (k)  and  associated  covariance 
elements  PJi  ,  PJ2  »  ?22 

2.  Compute  Kalman  gains 


Kl 


pTi 


2 

Pll  +  <*v 


Pl2 

K2  =  - 

2 

P12  +  °v 

3.  Update  estimate  with  measurement  z(k) 

xi(k)  =  xj~(k)  ♦  Ki(z(k)  -  x]~(k)) 

X2(k)  =  X2~(k)  +  K2(z(k)  -  xi~(k)) 

4.  Compute  error  covariance  for  updated  estimate 

pii  *  (i-ki)pTi 

P12  *  <l“Kl)pl2 
P22  s  -K2P12  +  P22 

5.  Project  ahead 

xi“(k+l)  =  xi(k)  +  Tx2(k)  +  A0m(k) 


(27) 


(28) 


(29) 
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X2  (k+1)  =  X2(k)  _  (30) 

PTl  =  Pll  *  2Tpi2  +  T2p22  +  qil 

Pl2  =  P12  +  ?P22  +  qi2 

P22  =  P22  +  q22  _ (31) 


6.  Set  k  =  k+1  and  go  to  step  2. 

3.2  Filter  Initialization.  The  filter  may  be  initialized  using  the  best 
estimates  of  the  system  state  available  from  prior  knowledge  of  the  process  and 
the  uncertainty  associated  with  it.  Thus  one  might  select 


’■!«’  0 

„  2 

-  0  °x20__ 

Here,  the  numbers  °xio»  °*20  would  be  small  when  xio»  *20  are  known  with 
a  high  degree  of  accuracy.  On  the  other  hand,  the  variances  would  tend  to 
infinity  when  one  has  very  little  confidence  in  the  choice  of  the  initial  values 


along  with 


P~(0) 


x10»  x20* 


Alternatively,  the  first  two  measurements  of  the  process,  z(0)  and 

z(l),  may  be  used  to  initialize  the  filter.  In  this  approach,  the  filter  would 

start  at  time  t=T  with  the  initial  estimates 

*f(l)  =  z(l)  (33) 

X2~(l)  =  z(l)  -  z(0) 

T  (34) 

The  error  covariance  associated  with  this  initialization  may  be  derived  using  the 
measurement  equations 

z(0)  =  xi(0)  +  v(0)  (35) 
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z(l  )  =  Xl( 1 )  +  v( 1 ) 

and  the  state  equations  (22)  which  yieLd 
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(36) 


x  i  ( 1 )  =  xi(0)  +  Tx2(0)  +  A9m(0)  +  W1(0)  _ (37) 

x2(l)  =  x2(0)  +  w2(0)  _ (38) 

From  (36)  and  (33), 

xi(l)  -  xi“(l)  =  -v(l) 

*  2 

and  hence,  E[(xi(l)  -  xi~(l))2]  =  E[v(l)2]  =  av  _ (39) 

Substitute  for  x2(0)  from  (37)  into  (38)  to  get 


xi(l)-xi(0)  A9m(0)  +  wi(0) 

x2(  1 )  =  -  -  -  +  w2(0) 


(40) 


Subtracting  (34)  from  (40), 


x2(l)  -  x2  (1)  = 


(xi(l)  -  z(l))  -  (xi(0)  -  z(0) )  A9m(0)  +  wi(0) 


+  w2(0) 


(41) 


Using  Eqns.  (35)  and  (36),  one  obtains, 

1 

x2(l)  -  x2“(l)  =  --(v(l)  -  v(0)  +  A6m(0)  +  wi(0))  +  w2(0) 

T 

and  hence,  _ (42) 


_  -  1 
E[(x2(1)-x2  (1))z]  =  — —  ♦  —  q1X  +  q22  -  -  qi2 


2CTV  1  2 


2 

T 


+  -  CT-  T 


(43) 


T*  3 

Also,  we  find, 

E[(xi(l)-xi-(l))(x2(l)-x2“( 1)] 
v(  1 ) 

=  E[  - (v(l)  -  v(0)  +  A9m(0)  +  wi(0)  -  Tw2(0)  )] 

T 
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Hence,  Che  filter  initialization  at  the  time  t=T  becomes, 


'(1)  = 


z( 1)  -  z(0) 
T 


P"(l)  = 


2ov  oa  T 


Note  that  cra  =  0  should  be  used  if  is  known  to  be  initially  non-accelerating. 
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4.  THREE-STATE  KALMAN  FILTER 
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4.1  Filter  Algorithm.  The  two-state  Kalman  filter  assumes  the  target 
acceleration  to  be  a  white  noise  process.  In  other  words,  the  acceleration 
represents  a  timewise  uncorrelated  random  function.  In  reality,  however,  the 
target  acceleration  would  generally  behave  closer  to  a  correlated  process.  A 
three-state  target  model  following  the  Singer  approach  is  developed  in  this 
seciton.  The  target  acceleraton  is  modeled  as  a  Gauss-Markov  random  process  and 
the  explicit  equations  forming  the  Kalman  filter  are  derived. 


Let  us  define  the  system  state  variables  as: 

*i  =  9t  -  6m  ,  tracking  error 
o 

X2  =  8t  ,  target  velocity 

oo 

X3  =  0t  ,  target  acceleration 

The  continuous  state  model  may  be  written  as, 
o 

*1  =  *2  " 
o 

*2  =  *3  _ 

o  /  r 

X3  =  -01x3  +v2aoa  n(t)  _ (47) 

where  b>m(t)  denotes  the  tracking  mount  rate.  The  last  equation  indicates  that 

the  target  acceleration  X3<t)  evolves  as  a  Gauss-Markov  process  with  correlation 

2  “Oil  T  | 

time  1/a.  The  process  X3(t)  has  the  autocorrelation  function  oa  e  and  n(t) 
represents  unity  white  noise  process. 

The  corresponding  discrete  time  state  equations  are  obtained  as 

x(k+l)  =  $(k)x(k)  +  u(k)  +  w(k)  _ (48) 
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1  T  $13 

$(k)  =0  1  $23 


0  0  $33 
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l  ~aT 

$13  =  _  (-1  +  aT  +  e  ) 

a2 

-aT 

$23  =  l-e  ) 
a 


$33  =  e 

The  vector  u(k)  is  simply  the  deterministic  input 


u(k)  =\  0 

\  0 


and  is  available  from  the  encoder  outputs 


A9m(k) 


A0m(k)  =  6m(k+l)  -  8m(k)  _ 

The  random  input  vector  w(k)  denotes  a  zero  mean  white  sequnce  with  the 


covariance 


where 


<111  <U2  qi3 


Q(k)  =  qi2  q22  q23 


qi3  q23  q33 


Oa  _2aT  * 

qil  =  — t  [l-e  +  2aT  +  - 
a4  3 


3  3 

2a  T  2  2  _aT 
-  -  2a  T  -AaTe  ] 


<*a  _2aT  _aT  _aT  2  2 

qi2  =  — =  [e  +  l-2e  +  2aTe  -  2aT  +  a  T  ) 
a3 


qi3  =  — 5  ~  2aTe  ] 
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q22  =  — =  [  4e  -  3  -  e' 

a 


aa  _2aT  _aT 

q23  =  -  [e  +  1  -  2e  ] 

a 

2  _2aT 

q33  =  [1-e  ] 


The  measurement  is  given  by 

z(k)  =  *i(k)  ♦  v(k)  _ (55) 

2 

with  R(k)  =  ov  ,  the  variance  of  the  sensor  error.  The  H(k)  matrix  simply  equals 
(1  0  0)  since  only  measurements  of  xi<k)  are  available.  Let  the  a  priori  and  a 
posteriori  estimation  error  covariances  be  denoted  as 


PI  1  P12  P13 

P“(k)  =  P12  P22  P23  *  Hk)  = 


P13  P23  P33 


Pll  P12  P13 


P12  P22  P23 


P13  P23  P33 


The  various  quantities  describing  the  discrete  three-state  model  may  now  be 
substituted  in  the  general  Kalman  filter  equations  of  chapter  2.  The  explicit 
filter  algorithm  for  the  three-state  Kalman  filter  then  becomes: 

1.  Obtain  initial  state  estimates  x]“(k),  X2~ (k),  X3~ (k)  and  the  associated 
estimation  error  covariance  elements  pJi  ,  pJ2  »  P13  >  P22  >  P23  >  P33* 

2.  Compute  Kalman  gains 


Pll  + 
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P12 

k2  = - 

2 

Pll  + 


P13 

k3  = - 

2 

Pll  +  ov 

3.  Update  estimate  with  measurement  z(k) 


xi(k)  =  xi  (k)  +  Ki(z(k)  -  xi  (k)) 


x2(k)  =  x2  (k)  +  K2(z(k)  -  xi  ( k ) ) 

*  *  A 

x3(k)  =  x3-(k)  +  K3(z(k)  -  xi~ (k)) 
4.  Compute  error  covariance  for  updated  estimate 
Pll  =  (l-Kl)pTl 

Pl2  =  ( l~Kl >pT2 

P13  *  (l“Kl)pl3 
P22  =  -K2Pl2  +  P22 
P23  =  -K2P"l3  +  P23 
P33  =  -K3Pl3  +  P33 


(57) 


(58) 


(59) 
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5.  Project  ahead 


xi  (k+1)  =  xi(k)  +  Tx2<k)  +  $13x3(0  +  A0m(k) 


X2  (k+1)  =  X2(k)  +  $23x3(k) 


X3  (k+1)  =  <<>33x3 (k) 


pTl  =  Pll  +  2Tpi2  +  2<fr13pi3  +  T^p22  +  2T$13p33  +  qU 

Pl2  =  P12  +  4»23P13  +  Tp22  +  <  ♦  1 3  +  T<J>23  >P23  +  $i3$23P33  +  qi2 

Pl3  =  $33<P13  ♦  TP23  ♦  $13P33>  +  qi3 

2 

P22  =  P22  +  2<*>23P23  ♦  <t>23P33  +  ^22 

P23  =  $33<P23  +  <t>23P33)  ♦  q23 
2 

P33  =  $33P33  +  q33  _ (61) 

6.  Set  k  *  k  +  1  and  go  to  step  2. 

4.2  Filter  Initialization.  The  three-state  Kalman  filter  may  be  initialized 
using  the  best  available  initial  position,  velocity  and  acceleration  states,  and 
the  associated  error  covariance.  One  might  use  the  initialization 


x  (0)  =  X20 


along  with 


P"(0)  = 


The  zero  initialization  for  the  acceleration  is  appropriate  because  it  is  modeled 


as  a  Gauss-Markov  process  which  is  a  zero  mean  process  in  the  ensemble  sense. 
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Alternatively,  the  first  two  measurement,  z(0)  and  z(l),  may  be  utilized  to 
initiate  the  filter  at  time  t=T.  Using  the  measurement  equations 
z(0)  =  xj,(0)  +  v(o) 

z(l)  =  x^O)  +  v(l)  _ (63) 

and  the  state  equations  (48)  to  yield 

xj(l)  =  xi(0)  +  Tx2(0)  +  $13*3(0)  +w^(0) 

X2(l)  =  X2(0)  +  (^23x3(0)  +w2(0) 

X3O)  =  $33x3(0)  +  W3(0)  _ (64) 

the  initial  error  covariance  may  be  derived  following  the  approach  detailed  in 
the  two-state  filter  case.  The  initial  state  estimate  is  thus 


x“(l) 


(65) 


and  the  elements  of  the  symmetric  P  (1)  matrix  are  given  by 


2 

Pll  (1)  =  ov 
2 

P12  (1)  = 

T 

P13  ( 1 )  =  0 


2  2 

2av  oa  2  2  2  3  3  _aT  _aT 

P22  (1)  =  -  +  -  [2-a  T  +  O  T  -  2e  -  2aTe  ] 

T  Ct  T  * 


Oa2  OtT 

P23  (1)  *  —5 -  [e  +  aT  -  1] 

CTT 

P33~ (1)  =  °a2  _ (66) 
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Note  that,  oa  =  0  should  be  used  if  the  system  is  known  to  be  non¬ 
accelerating  initially. 
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5.  FILTER  ADAPTION  AND  MANEUVER  DETECTION 


The  Kalman  filter  development  presented  in  the  previous  chapters  assumes  a 

knowledge  of  the  target  dynamic  characteristics  as  well  as  the  measurement 

uncertainty.  However,  in  practice,  the  information  concerning  the  target 

2 

acceleration  variance  ca  may  be  limited.  This  is  particularly  true  in  case  of 
targets  that  are  known  to  exhibit  maneuvering  dynamics. 

The  target  dynamics  uncertainty  enters  the  filter  algorithm  through  the  matrix 
Q(k).  Lacking  a  precise  value  of  Q(k),  it  may  be  viewed  as  a  parameter 
influencing  the  filter  design.  Increasing  the  elements  of  Q(k)  results  in  larger 
values  of  the  a  priori  estimation  error  covariance  P~(k)  as  per  Eqn  (20).  When 
the  elements  of  Q(k)  (and  hence  those  of  P~(k))  are  large  compared  to  those  of 
R(k) ,  the  Kalman  gains  K(k)  assume  larger  values  as  indicated  by  Eqn  (16).  The 
filter  thus  puts  a  relatively  greater  weight  on  the  new  measurement  than  on  the  a 
priori  estimate.  Relatively  more  measurement  noise  is  then  present  in  the 
filtered  estimate  x(k).  The  algorithm  therefore  represents  a  wideband  filter. 
Conversely,  when  the  elements  of  Q(k)  are  relatively  small  compared  to  those  of 
R(k),  The  Kalman  gains  K(k)  become  small.  The  algorithm  then  puts  more  weight  on 
the  a  priori  estimate  x~(k)  compared  to  the  new  measurement  in  forming  the 
filtered  estimate  x(k).  In  other  words,  it  relies  more  on  the  past  history  and 
the  state  projection  via  the  state  transition  matrix  4>(k).  Therefore,  less  noise 
is  present  in  the  filtered  estimate  and  the  algorithm  represents  a  narrowband 
filter. 
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In  view  of  the  above  a  narrowband  filter  design  would  appear  more  desirable. 
However,  if  the  true  uncertainty  in  the  target  dynamics  exceeds  the  Q(k)  value 
used  in  the  filter  design,  the  narrowband  characteristic  (small  Q(k)  to  R(k) 
ratio  and  hence  small  K(k))  would  cause  too  much  of  the  signal  contained  in  the 
observations  z(k)  to  be  rejected.  The  filtered  estimate  may  then  fail  to  follow 
the  true  motion  and  may  even  diverge  from  it.  Consequently,  a  judicious 
selection  of  the  filter  bandwidth  is  clearly  indicated. 


The  effect  of  a  mismatch  in  modeling  has  a  similar  effect.  When  the  state 
transition  matrix  <J>(k)  assumed  in  the  filter  design  deviates  from  the  true  model 
of  the  system,  the  projected  estimate  x~(k)  would  be  in  greater  error  than 
indicated  by  its  covariance  matrix  P-(k).  Again,  the  possibility  of  divergence 
due  to  this  reason  may  be  reduced  by  incorporating  an  increased  Q(k)  leading  to  a 
wideband  filter. 


The  ability  to  control  the  filter  bandwidth  by  varying  the  Q(k)  to  R(k)  ratio  may 
be  used  to  track  maneuvering  targets.  Suppose  a  good  narrowband  filter  has  been 
designed  for  a  known  type  of  target  motion.  If  the  target  makes  a  sudden 
maneuver,  the  narrowband  filter  may  be  too  slow  to  react  to  it  because  of 
the  less  weightage  on  the  new  observation  and  the  track  may  be  lost.  One  way  of 
correcting  the  situation  would  be  to  increase  the  filter  bandwidth  as  soon  as  the 
maneuver  occurs  by  increasing  Q(k).  When  the  maneuver  stops,  Q(k)  may  be  reduced 
to  realize  a  narrowband  filter  again  and  achieve  better  filtering.  This  would 
represent  an  adaptive  technique  for  tracking  maneuvering  targets.  However,  a 
maneuver  detection  method  is  required. 

A  maneuver  may  be  detected  by  monitoring  the  innovations  process 
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v(k)  =  z(k)  -  H(k)x“(k)  _ (67) 

The  innovations  process  represents  a  zero  mean  process  with  the  covariance 

S(k)  =  H(k)P“(k)HT(k)  +  R(k)  _ (68) 

Assuming  a  Gaussian  distribution  for  the  components  of  v(k),  the  probability  that 
each  component  V£  lies  in  the  interval  +  c  ( i , i )  where  c  is  a  positive  constant 
is  well  known.  A  simple  maneuver  detection  scheme  may  thus  be  formed  by  checking 
each  V£(k)  against  this  interval.  Whenever  any  of  the  V£(k)  falls  outside  the 
range  a  maneuver  may  be  declared. 


An  alternative  approach  is  to  consider  the  normalized  random  variable  p(k) 
defined  as 


p  (k)  =  uT(k)S_1(k)v(k)  _ (69) 

It  can  be  shown  that  p(k)  has  a  chi-squared  distribution  with  m  degrees  of 
freedom  where  m  is  the  dimension  of  the  v(k)  vector.  Hence, 

E[  p(k)  ]  =  m  _ (70) 

The  criterion  p(k)  >  cm,  where  c  >  1  is  a  constant,  may  then  be  used  to  detect  a 
maneuver.  Or,  a  fading  memory  average  computed  from 

g(k)=  Yg(k_1)  ♦  pOO*  0  <  y  <  1  _ (7D 

may  be  used.  Note  that 


m 


Lim  E[g(k) ]  =  l-y 

k— >®  _  (72) 


Thus, 


whenever  g(k)  exceeds  c  m  , 

(1-Y> 


a  maneuver  may  be  declared. 


Once  a  maneuver  is  detected  using  either  of  the  above  techniques,  the  Q(k)  matrix 
may  be  increased  suitably.  This  would  widen  the  filter  bandwidth  enabling  it  to 
track  the  maneuvering  target. 


6.1  General .  The  earlier  sections  addressed  the  development  of  Kalman  filter 
algorithms  assuming  a  single  sensor  providing  noisy  measurement  of  the  position 
misalignment  between  the  target  and  the  tracking  mount.  When  a  single  target  is 
to  be  tracked  using  a  set  of  multiple  sensors  providing  noisy  measurements  of  the 
tracking  error,  the  sensor  outputs  may  be  processed  using  either  a  parallel  or  a 
centralized  architecture. 


6.2  Parallel  Architecture.  The  parallel  processing  architecture  is  schematically 
shown  in  Figure  3.  Each  sensor  output  z1  is  processed  by  a  Kalman  filter  which 
provides  the  optimal  state  estimate  x1  and  the  associated  estimation  error 
covariance  matrix  P1 ,  i  *  1,4.  The  individual  Kalman  filter  estimates  are  then 
optimally  combined  to  form  the  single  state  estimate  x  and  its  error  covariance 
matrix  P. 


The  track  fusion  relationships  may  be  developed  readily  using  the  minimum 
expected  Mean  Square  Error  (MSE)  criterion.  To  simplify  the  approach,  let  us 
consider  the  scalar  problem  of  optimally  combining  the  four  Kalman  filter 
estimates  of  the  tracking  error.  Further,  we  introduce  the  simplified  notion: 
y  =  true  value  of  xj, 

*  (i) 

yi  =  xi  ,  i  =  1,4 

2  (i) 

®i  *  Pll 
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Let  the  combined  estimate  be  expressed  as, 

yc  =  cm  ♦  C2Y2  +  c3y3  +  C4y4 

The  estimation  error  then  becomes, 

ec  =  y-yc 


=  y  -  ciyi  -  C2y2  *  C3y3  -  C4y4 
=  ci(y-yi)  +C2(y~y2>  +  C3(y-y3>  +  C4(y-y4> 
+  (1  -  ci  -  C2  -  03  -  C4>y 
=  ciej  +  C2e2  +  C3e3  +  0464 
+  (1  -  ci  -  C2  “  03  -  C4>y 
For  an  unbiased  estimate,  one  requires 
E[ec]  =  0 

Hence, 

C1  +  c2  +  c3  +  C4  -  1  =  0 

which  leads  to 

ec  =  ciex  +  C2e2  +  0303  +  0464 


TM-3628 


(74) 


(75) 


(76) 

(77) 

(78) 


2  22  22  22  22 

E[ec  ]  =  ci  Ox  +  C2  02  +  c3  °3  +  c4  °4 

2 

Minimization  of  E[ec  ]  subject  to  the  constraint  (77)  results  in 

1  1 
ci  =  -  •  - 


The  optimal  combined  tracking  error  estimate  becomes 


(79) 


(80) 


(81) 
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*  (c)  1 

XI  =  - 


r^1 

L — /Pn 
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1  “  (i) 

-  XI 


E— 


Subsituting  Eqn  (74)  into  Eqn  (73),  the  variance  of  the  combined  tracking 
position  estimation  error  becomes 


2  1 


6.3  Centralized  Architecture.  In  the  centralized  architecture,  the  measurements 
from  all  the  sensors  are  first  combined  optimally  to  produce  a  single  equivalent 
measure  of  the  track  misalignment.  A  single  Kalman  filter  then  processes  the 
combined  measurement  data  to  produce  an  optimal  estimate  of  the  system  state. 
Figure  4  shows  a  schematic  diagram  illusrating  the  centralized  architecture. 


To  obtain  the  optimal  sensor  data  fusion  relation,  express  the  desired  combined 
measurement  z^c^  as  a  linear  combination  of  the  individual  sensor  measurements  of 


z(c)=  ciz^^  +  C2z^^  +  C3z^^  +  C4z^^  _ (84] 

The  equivalent  measurement  equation  used  by  the  Kalman  filter  may  be  written  as 
z(c)  =  xi  v(c)  _ (85] 
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The  error  in  Che  equivalent  measurement  is  given  by 


v(c)  =  z(c)  _ 


*1 


=  ci(z^^  -  xj)  +  C2(z^^  -  x^)  +  03(2^  -  xi) 
+  C4<z^^  -  xi)  +  (ci  +  C2  +  C3  +04  -l)xi 
=  civ^^  +  C2v^2^  +  C3v^^  +  C4v^^ 

+  (ci  +  02  +  C3  +  04  -  l)xi 


(86) 


As  done  earlier  in  the  parallel  architecture  case,  the  requirement  of  an  unbiased 
estimate  and  the  minimization  of  the  error  variance  leads  to 

1  1 


c,  = 


2  r — v  1 


(87) 


2 
vi 

which  gives  the  optimum  data  fusion  relations 

1 


,<c) 


E~2  v 


°vi 


(88) 


(89) 


Ci  — 

The  equivalent  measurement  and  its  error  variance  thus  obtained  are  processed  by 
the  single  Kalman  filter  to  produce  the  optimal  state  estimate. 


6.4  Processing  Hardware  Design 

6.4.1  General .  The  microcontroller  accepts  multiple  sensors  input  information 
and  processes  the  data  based  on  the  above  control  algorithms.  The  results  of  the 
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processing  are  output  in  the  form  of  control  command  signals  capable  of  driving  a 
tracking  and  pointing  system. 


The  microcontroller  is  composed  of  several  subsystems.  The  subsystem  functions 
provide  for  user  interface,  shared  RAM,  sensor /signal  input  and  processing,  and 
output  control.  Figure  5  shows  a  block  diagram  of  a  typical  microcontroller  and 
the  various  subsystems  and  functions. 


The  user  interface  consists  of  a  keyboard/terminal  for  user  interaction.  The 
user  interface  provides  the  means  by  which  the  operator  can  interact  with  the 
microcontroller  using  a  high  level  programming  language  to  vary  the  control 
parameters  of  the  pointing  system.  The  level  of  interaction  will  allow  the  user 
to  develop  and  implement  the  necessary  algorithms. 

The  Central  processing  Unit  (CPU)  is  a  general  purpose  subsystem  used  in  the 
controller  to  implement  the  Kalman  filter  and  optimal  combining  algorithms.  A 
block  diagram  of  the  CPU,  Motorola  Part  Number  MVME133-1,  is  presented  in  Figure 
6.  The  main  features  of  the  CPU  system  are  microprocessors,  cache  memory,  memory 
management,  timing,  interrupt  handling,  and  serial  I/O.  The  CPU  subsystem 
combines  the  processing  power  of  the  Motorola  32-bit  MC68020  Microprocessor  with 
the  speed-enhancement  properties  of  a  16Kb  Cache  Memory  Accelerator.  The  Cache 
Accelerator,  a  small,  fast  memory  system,  compensates  for  the  typical  speed 
mismatch  between  a  very  fast  CPU  and  its  relatively  slow  associated  dynamic  RAM 
main  memory  system.  It  does  so  by  concurrently  storing  the  data  most  recently 
stored  in  main-memory  locations.  These  data  can  subsequently  be  obtained  by  fast 
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accesses  of  "cached"  locations  rather  than  by  much  slower  accesses  of  main 
memory. 


To  accommodate  for  the  fact  that  the  cache  memory  normally  has  much  less  capacity 
than  the  main  memory  system,  little-used  accesses  stored  in  the  cache  memory  are 
routinely  replaced  with  more  active  ones,  based  on  the  bus  traffic  between  the 
Microprocessor  Module  and  the  bus  and  memory  interfaces.  This  assures  maximum 
utilization  of  the  cache.  The  microprocessor  operates  at  a  fixed  frequency  of  16 
MHz.  To  further  improve  the  processing  speed  and  capabilities  of  the  CPU  system, 
the  microprocessor  is  supported  by  a  Motorola  MC68881  floating  point  coprocessor. 
Commercial  grade  CPU  systems  are  available  which  operate  with  clock  frequencies 
up  to  24  MHz.  The  MC68020  can  interface  and  control  as  many  as  four  68881 
coprocessors. 

The  memory  management  function  implements  demand-paged  virtual  memory  operations. 
It  provides  the  required  logical  to  physical  address  translation  by  performing 
searching  of  translation  tables  in  main  memory. 

The  onboard  programmable  timer  provides  three  independent  cascadable  16-bit 
counters  with  interrupt  capability. 

The  interrupt  handler  allows  interrupts  to  the  onboard  CPU  from  up  to  20  sources. 
The  interrupt  handler  preprocesses  interrupt  sources  into  three  groups  of  seven 
interrupts  corresponding  to  the  seven  possible  MC68020  interrupt  levels.  The 
groups  are  labeled  Group  1,  Group  2,  and  Group  3.  The  interrupt  service  priority 
is  determined  by  the  interrupt  level  and  the  group  number.  Interrupts  with 
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different  interrupt  levels  are  processed  according  to  the  standard  interrupt 
processing  discipline.  Interrupts  within  an  interrupt  level  are  processed 
according  to  the  group  number. 


Group  1  is  reserved  for  system  bus  interrupts.  The  interrupt  handler  processes 
Group  2  and  3  interrupts  differently  from  Group  1  interrupts.  If  the  interrupt 
being  acknowledged  is  a  Group  2  or  3  interrupt,  the  interrupt  handler  fetches  the 
appropriate  exception  vector  number  from  PROM  and  sends  it  to  the  CPU  via  the 
local  bus.  If  the  interrupt  being  acknowledged  is  a  Group  1  interrupt,  the 
exception  vector  number  is  fetched  from  the  system  bus  where  it  was  placed  by 
the  interrupting  device. 

The  serial  I/O  ports  are  accessible  via  a  50-pin  flat  ribbon  cable  connector  at 
the  top  of  the  board.  Signal  levels  at  this  connector  correspond  to  TTL 
specifications,  but  can  be  transformed  to  RS-232-C  levels  by  means  of  a  separate 
distribution  board  and  cable  assembly.  The  ports  have  full-  and  half-duplex 
compatibility  with  programmable  baud  rates.  They  are  suitable  for  synchronous  or 
asynchronous  operation,  with  5  to  8  bits  per  character,  plus  parity. 

The  sensor/signal  input  and  processing  consists  of  parallel  digital  input  ports, 
analog-to-digital  converters,  and  television  frame  signal  processers.  It  is 
expected  that  the  existing  hardware  will  be  useable  in  the  new  controller.  These 
inputs  are  made  available  for  processing  in  the  new  CPU  assembly. 
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The  output  control  subsystem  provides  the  interface  between  the  processed  data  of 
the  signal  input/processing  subsystem  or  the  user  command  data  from  the  user 
interface  subsystem  to  drive  the  control  command  signals  for  pointing  control. 


6.4.2  Processing  Time  Requirements.  The  estimated  processing  time  to  compute  the 
state-variable  filter  and  combining  algorithms  are  presented.  Worst  case 
analysis  is  presented  which  ignores  the  speed  increases  available  through 
the  inherent  pipelining  capabilities  of  the  suggested  CPU  with  cache  RAM  and  math 
coprocessors.  Also,  a  16  MHz  clock  is  assumed. 


Table  6-1  presents  the  amount  of  double  precision  floating  point  calculations 
required  for  filtering  and  combining  one  set  of  data.  The  two-state  Kalman 
filter  computation  requirements  are  based  on  the  algorithm  presented  in  Section 
3.1.  Section  4.1  describes  the  three-state  Kalman  filter.  The  optimal  combining 
algorithms  are  presented  in  Sections  6.2  and  6.3. 


TABLE  6-1.  FLOATING  POINT  COMPUTATION  REQUIREMENTS 


ALGORITHM 

ADD 

SUBTRACT 

1 

MULTIPLY 

DIVIDE 

JLL 

PROCESSING  TIME 

2-STATE  FILTER 

13 

5 

1 

10 

2 

II 

274.0  MSEC 

3-STAT&  FILTER 

34 

7 

1 

37 

3 

II 

744.5  MSEC 

PARALLEL  COMBINING 

6 

1  o 

1 

5 

5 

II 

158.5  MSEC 

CENTRAL  COMBINING 

6 

1  0 

1 

5 

5 

II 

158.5  MSEC 

The  estimated  time  for  performing  each  math  computation  listed  in  Table  6-1  is 
given  below.  This  assumes  double-precision,  floating  point  calculations  using  a 
68020  CPU,  68881  math  coprocessor,  a  16  MHz  clock,  and  a  32-bit  wide  data  bus. 


FADD 

136 

CLOCK 

CYCLES 

8.5 

MSEC 

FSUB 

136 

CLOCK 

CYCLES 

8.5 

msec 

FMUL 

156 

CLOCK 

CYCLES 

9.75 

msec 

FDIV 

188 

CLOCK 

CYCLES 

11.75 

msec 
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Methods  available  to  easily  improve  the  required  processing  time  includes  using 
faster  system  clocks  which  would  provide  up  to  a  33X  increase  and  using  multiple 
math  coprocessors.  Although  the  68020  can  physically  interface  with  eight  math 
coprocessors,  the  factory  states  that  four  coprocessors  per  CPU  is  a  practical 
limit.  The  four  coprocessors  would  improve  the  processing  time  by  about  50Z. 
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7.  COMPUTER  SIMULATION 


7.1  Simulation  Program  Description.  A  computer  program  has  been  developed  to 
evaluate  the  filtering  techniques  presented  in  the  study.  The  program  simulates 
the  target  motion,  measurement  noise  and  the  filtering  process.  Either  the 
two-state  or  the  three-state  Kalman  filter  algorithm  may  be  simulated. 


The  target  motion  is  assumed  to  be  governed  by  the  state  equations: 
o 

xi  =  x2 


x2  =  X3  +  a(t ) 


*3  =  “6*3  +  y28oa  *  n(t)  _ (90) 

where  x^  =  target  position,  x2  =  target  velocity.  The  target  acceleration  is 
composed  of  a  deterministic  part  a(t)  and  a  random  component  X3.  The  random 
component  is  modeled  as  a  Gauss-Markov  process  with  the  autocorrelation  function 
2  ~B I T | 

°a£e  .  The  function  n(t)  represents  unity  amplitude  white  noise  process. 

The  discrete  equivalent  of  these  continuous  target  state  equations  used  by  the 


program  is 


y(k+l)  =  $(k)y(k)  +  u(k)  +  w(k) 


Here  u(k)  denotes  the  deterministic  input  vector 

/LA 


u(k)  =  T  a(k) 
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The  state  transition  matrix  $(k)  and  the  zero-mean  white  sequence  w(k)  are 
described  by  equations  of  section  4.1  with  Cl  replaced  by  B»  and  oa  replaced  by 


a 


at  • 


The  random  numbers  forming  the  sequence  w(k)  are  generated  using  a  random  number 

subroutine.  The  subroutine  generates  random  numbers  x  uniformly  distributed 

between  0  and  1.  Zero  mean  random  numbers  y  uniformly  distributed  in  the  range 

-A  to  +A  are  then  obtained  through  the  transformation  y  =  A(2x  -  1).  The  numbers 

2  A2 

y  have  the  variance  In  Ctie  Present  Pr08ram*  c^e  “(k)  inputs  are  zero-mean 

sequences  with  specified  covariance  Q(i,j).  The  program  thus  uses  A£  =^3q£j  to 
simulate  the  appropriate  random  numbers  representing  the  elements  of  the  w(k) 
vector. 


The  measurement  is  modeled  by  adding  a  random  noise  sequence  v(k)  to  the  true 
target  position  xjtk)  obtained  as  the  solution  of  the  target  state  equations. 

The  measurement  noise  is  assumed  to  be  a  uniformly  distributed  zero-mean  sequence 


with  variance  ov.  This  is  achieved  by  letting  A  =  ^3  ov  in  the  random  number 

generator  computation.  The  program  permits  the  input  of  piecewise  constant 

2 

values  of  the  measurement  variance  Ov. 


Either  the  two-state  or  the  three-state  Kalman  filter  algorithm  may  be  simulated. 
The  filter  may  be  initialized  either  using  a  priori  state  estimates  and  their 
associated  error  covariances,  or  using  the  first  two  measurements  to  generate 
these  initial  quantities. 
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The  simulation  also  provides  the  option  of  evaluating  the  adaptive  filtering 
approach  using  the  innovations  process.  This  enables  the  filter  to  achieve  a 
wider  bandwidth  whenever  a  maneuver  is  detected. 


All  quantities  of  interest  concerning  the  target  motion,  measurement  and 
filtering  errors,  error  covariance,  Kalman  gains  etc.  can  be  printed  out.  The 
mean  and  mean  square  values  of  the  measurement  errors  simulated  during  a  run  are 
printed  out  to  provide  a  check  on  the  random  number  generation.  The  mean  and 
mean  square  values  of  the  filtered  position  and  velocity  errors  during  a  run  are 
printed  out  to  provide  a  measure  of  the  effectiveness  of  the  filter. 

The  program  is  presently  limited  to  the  input  of  a  pulse-type  acceleration 

profile  for  the  deterministic  component  a(t)  of  the  target  acceleration.  The 

2 

measurement  error  variance  av  also  is  restricted  to  five  constant  values  during 
one  run.  These  restrictions,  however,  are  minor  in  nature  and  may  be  removed 
quite  readily.  The  program  currently  performs  a  single  run.  On  the  other  hand, 
Monte  Carlo  methods  should  be  used  to  fully  establish  the  performance  of  the 
filter  design  under  consideration.  It  would  therefore  be  desirable  to  enhance 
the  capability  of  the  program  to  perform  multiple  runs  and  output  the  ensemble 
statistics . 


7.2  Simulation  Results.  The  results  of  a  few  typical  computer  simulation  runs 
are  presented  in  this  section.  A  comment  concerning  the  units  of  the  various 
quantities  of  interest  would  be  appropriate.  Any  time  unit,  such  as  second, 
minute,  may  be  associated  with  the  time  scale  values.  Similarly,  any  position 
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unit  such  as  degrees,  radians,  feet  etc.  may  be  associated  with  the  position 
scale.  The  units  of  the  various  quantities  of  interest  then  become: 
measurement  [position  unit] 

[position  unit] 

[position  unit/(time  unit)] 

[position  unit/(time  unit)^] 


position 

velocity 

acceleration 


Kalman  gain  K1  [dimensionless] 

Kalman  gain  K2  [l/(time  unit)] 

Kalman  gain  K3  [l/(time  unit)^] 


The  selection  of  the  filter  parameters  requires  some  knowledge  of  the  process, 

i.e.,  the  target  dynamics.  Suppose  it  is  known  that  the  target  may  have  any 

acceleration  with  an  equal  probability  in  the  range  of  -60  to  +60.  This  yields 

2 

the  acceleration  variance  aa  =  1200.  Let  the  best  estimate  of  the  acceleration 
correlation  time  be  about  100  time  units,  i.e.,  a  *  0.01.  These  parameters  are 
used  to  define  the  two-  and  three-state  Kalman  filters  for  the  simulation. 


The  filters  may  be  initialized  using  either  of  the  two  approaches  discussed 
earlier.  Here  we  assume  an  initial  position  value  of  3.0  with  an  associated 
variance  of  25.0  representing  the  accuracy  of  the  sensor  system.  Assuming  an  a 
priori  knowledge  that  the  initial  velocity  may  be  any  value  in  the  range  -60  to 
+60  with  uniform  probability  yields  a  variance  of  1200.  Using  the  mean  values  as 
the  best  initial  estimates,  the  three-state  filter  is  thus  initialized  as 
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25  0  0 


P-(O)  =  0  1200  0 


0  0  1200 


The  initialization  used  for  the  two-state  filter  is 


x— (0)  = 


P“(0)  = 


25  0 


0  1200 


Unknown  to  the  Kalman  filter,  we  consider  a  target  motion  consisting  of  a 
constant  acceleration  of  40.  Superposed  on  this  is  a  random  acceleration 
uniformly  distributed  between  -8.66  to  8.66  which  corresponds  to  a  variance  of 
25.  The  random  acceleration  is  assumed  correlated  with  8  =  1.0. 


The  measurements  are  assumed  to  be  of  varying  accuracy  over  the  time  period  of 
interest.  Uniformly  distributed  measurement  errors  with  variances  of 

2 

av  =  25  (0  _<  t  <  2) 

=  1  (2  <  t  <  4) 

=  49  (4  <  t  <  6) 

=  4  (6  <  t  <  8) 

=  16  (8  <  t  <  10) 

were  arbitrarily  selected  for  the  simulation.  Note  that  this  corresponds  to  the 
measurement  errors  being  uniformly  distributed  in  the  range  -Ji  ov  to  ov. 

The  sampling  rate  was  assumed  to  be  100  samples  per  unit  time,  i.e.,  T  =  0.01 


time  units. 
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Figure  7  shows  a  plot  of  the  true  target  position  s}  as  a  function  of  time.  The 
measurement  error  at  each  sampling  point  is  shown  in  Figure  8.  Joining  the 
successive  points,  the  plot  in  Figure  9  shows  a  better  perspective  of  the 
measurement  errors  as  the  data  arrive  sequentially.  The  errors  generated  by  the 
program  confirm  to  the  specified  variation  of  the  measurement  accuracy. 


The  output  of  the  two-state  filter  is  presented  first.  Figure  10  shows  the 

variation  of  the  filtered  position  error  as  a  function  of  time.  A  comparison 

with  the  measurement  error  time  history  clearly  shows  the  effectiveness  of  the 

Kalman  filter  in  reducing  the  effect  of  measurement  noise.  Over  the  time  period 

of  10  units,  the  rms  position  error  is  found  to  be  1.4210  in  contrast  to  the  rms 

measurement  error  of  4.3589.  The  filtered  velocity  output  is  shown  in  Figure  11 

and  the  rms  velocity  error  over  10  time  units  is  11.4643.  Figure  12  shows  the 

variation  of  the  Kalman  position  gain  Kj.  It  is  apparent  that  the  gain 

automatically  adjusts  in  accordance  with  the  quality  of  the  measurements.  For 

2 

example,  at  t=2,  the  measurement  quality  changes  from  a  variance  of  ov  =  25 
to  1.  The  filter  assumes  a  much  bigger  gain  Ki  and  thus  puts  a  substantially 
higher  weight  on  the  new  data  in  comparison  to  the  prediction  from  the  state 
model.  This  follows  a  transient  period  during  which  the  model  predictions 
successively  improve  and  the  weightage  on  the  new  observations  reduces. 

Finally,  a  steady  state  is  arrived  when  K^  assumes  a  constant  value.  The 
variation  of  the  velocity  gain  K2  showing  similar  behavior  is  presented  in  Figure 
13. 


A  measure  of  the  quality  of  the  filter  design  may  be  obtained  from  the  diagonal 
elements  of  the  estimation  error  covariance  matrix  P(t).  Plots  of  the  rms 
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position  and  velocity  estimation  error,  J~?\l  and  yj?22  >  respectively,  are  shown  in 
Figures  14  and  15.  These  represent  the  theoretical  values  of  the  filtering  error 
statistics  and  are  meaningful  in  an  ensemble  sense.  Furthermore,  these 
correspond  to  the  ideal  situation  when  the  system  state  model  used  in  the  filter 
and  the  truth  model  are  identical.  Again,  the  effect  of  the  measurement  quality 
variations  on  these  statistics  is  apparent. 


The  results  obtained  with  the  three-state  filter  are  presented  next.  Figure  16 
shows  the  position  filtering  error.  It  shows  an  rms  value  of  0.8320  over  the  10 
time  units  of  simulation.  The  velocity  error,  with  an  rms  value  of  7.6585  over 
the  simulation  period,  is  shown  in  Figure  17.  The  three-state  filter  estimates 

9 

the  acceleration  with  the  error  presented  in  Figure  18.  The  Kalman  gains  Ki,  K2 
and  K3  are  shown  in  Figure  19,  20,  and  21,  respectively.  The  theoretical 
position,  velocity  and  acceleration  error  statistics  associated  with  this  filter 
design  are  presented  in  Figures  22,  23,  and  24,  respectively. 

From  the  above,  it  would  appear  that  the  three-state  filter  performs  better  than 

the  two-state  filter.  However,  due  to  the  statistical  nature  of  the  problem,  a 

single  simulation  suggests  nothing  of  value  regarding  a  comparison  between  the 

two  situations.  In  fact,  the  performance  of  each  filter  is  highly  dependent  on 

the  nature  of  the  target  motion  and  the  tuning  of  the  filter  via  the  selection  of 
2 

Oa,  the  correlation  characteristic  a  and  the  sampling  interval  T.  As  a  general 
feature,  however,  reducing  the  sampling  interval  would  improve  the  filtering 
performance  in  both  cases. 
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Figure  2.  Kalman  Filter  Loop 
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Figure  3.  Schematic  Representation  Of  Parallel  Architecture 
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Figure  4.  Schematic  Representation  of  Centralized  Architecture 


TM-3628 


43 


Figure  5.  Typical  Microcontroller  Block  Diagram 
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Figure  6.  CPU  Block  Diagram 
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Figure  7.  Simulated  Target  Position  Versus  Time 
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Figure  8-  Simulated  Measurement  Error  Versus  Time 


Figure  12.  Kalman  Gain 


Figure  18-  Filtered  Accelerati 
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APPENDIX  A 

ALGORITHM  SIMULATION  COMPUTER  PROGRAM 
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The  FORTRAN  listing  of  the  computer  simulation  program  are  enclosed  in  this 
appendix.  The  program  input  and  a  sample  output  file  are  also  given.  The 
various  input  quantities  are: 

T  =  Sampling  period 
ALPHA  =  a 
SIA  =  Oa 
SIM  =  av 

TMAX  =  Value  of  time  for  program  termination 
SIAM  =  °ac 
BETA  =  B 

XINL  =  Initial  target  position 
VINL  -  Initial  target  velocity 
AINRAND  =  Initial  value  of  random  target  acceleration 
AMEAN1  =  Constant  target  acceleration  outside  the  range  Tj  <  t  <  T2 
AMEAN2  ■  Constant  target  acceleration  in  the  range  Ti  <  t  <  T2 
XSTART  =  x!-(0) 

VSTART  =  X2~(0) 

ASTART  =  X3~(0) 

ISEED  *  Random  number  generator  seed  (odd  integer) 

INITIAL  =  0  for  filter  initialization  using  first  two  measurements 
=  nonzero  for  alternative  initialization 
ISTATE  =  2  for  simulating  two-state  filter 

=  3  for  simulating  three-3tate  filter 
GAMMA  =  Y 

GMAX  =  max  value  of  g(*)  for  maneuver  detection 


A-2 
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QF ACTOR 
AA 
BB 
CC 
DD 


factor  by  which 


Value 

of 

ov 

for 

Value 

of 

°v 

for 

Value 

of 

<JV 

for 

Value 

of 

ov 

for 

Q(k)  matrix  is  increased  upon  detection  of  maneuver 
2  <  t  <  4 

4  <  t  <  6 

6  <  t  <  8 

8  <  t 


A- 3 


r>  ooon  o  o  o  n  oo 


KALMAN  FILTER  SIMULATION  PROGRAM 

IMPLICIT  REAL«8  (A-H, O-Z) 

REAL  *4  YFL 

DIMENSION  XM<3)»  X<3),  P<3.  3),  ZM<2> 

DIMENSION  PM  <  3-  3),  PHI  (3.  3).  Q(  3,  3) 

DIMENSION  S  (  3 ) ,  SS < 3 ) > STM (3,  3),  QG<3, 3) 

CHARACTER  *  80  I BUFF 

COMMON  /  CARDCOMMON  /  I  BUFF,  ICARDNUM,  IOUTCHAN,  INCHAN 
INCHAN  =  2 
IOUTCHAN  =  3 
ICARDNUM  =  0 

OPEN  <2, FILE= 'KAL.  IN  ' , STATUS= 'OLD ' ) 

OPEN  (3, FILE= 'KAL.  OUT',  STATUS= 'NEW ' ) 

CALL  RDLINE 
CALL  GETDFP  < 1 ,  T,  0) 

CALL  GETDFP <2,  ALPHA,  0) 

CALL  GETDFP  <3, SIA,  0) 

CALL  GETDFP<4,  SIM,  0) 

CALL  GETDFP < 5,  TMAX, O) 

CALL  RDLINE 

CALL  GETDFPI1, SIAM,  O) 

CALL  GETDFP (2,  BETA,  0) 

CALL  GETDFP<3,  XINL, 0) 

CALL  GETDFPI4,  VINL,  0) 

CALL  GETDFP < 5,  AINRAND,  0) 

CALL  RDLINE 

CALL  GETDFP < 1,  AMEAN1, 0) 

CALL  GETDFP (2,  Tl,  0) 

CALL  GETDFP < 3,  T2,  0) 

CALL  GETDFP < 4,  AMEAN2,  0 ) 

CALL  RDLINE 

CALL  GETDFP ( 1 ,  XSTART , O ) 

CALL  GETDFP (2, VSTART,  O) 

CALL  GETDFP (3, ASTART, O) 

CALL  RDLINE 

CALL  GETINT ( 1, ISEED, 0) 

CALL  GETINT(2,  INITIAL, O) 

CALL  GETINT(3, ISTATE, O) 

CALL  RDLINE 

CALL  GETDFP ( 1,  GAMMA,  0) 

CALL  GETDFP (2i  GMAX, O) 

CALL  GETDFP<3, QFACTOR, 0) 

CALL  RDLINE 

CALL  GETDFP ( 1, AA, 0) 

CALL  GETDFP  <  2,  BB,  0) 

CALL  GETDFP ( 3,  CC,  0) 

CALL  GETDFP (4, DD, 0) 

AT  =  ALPHA  *  T 
ET  =  DEXP(-AT) 

EET  =  DEXP  < -2.  *  AT) 

SASQ  =  SIA  *  SIA  , 

R  =  SIM  *  SIM 

INITIALIZE  PM  MATRIX  <  ERROR  COVARIANCE  MATRIX) 


lt-i  ini  i  ihl.  tu.  u  )  i ntn 

PM(  1,  1  )  =  R 

PM(  1,  2)  =  R  /  T 

PM(  1,  3)  =  0. 

PP  =  SASQ/(ALPHA**4.  *  T*T) 

PPP  =  2.  -  AT*AT  +  2.  *AT**3.  /3  -2.  *ET  -2  *AT*ET 

PM(2,  2)  =2.  *R/(T*T)+PP*  PPP 

IF  (  ISTATE.  EQ.  2)  PM(2,  2)  =  2.  *  R  /(T  *  T )  -r  SASQ  *T/3. 

PM(2<  3)  =  SASQ*  (  ET  +  AT  -1.  )  /  ( ALP HA* ALP HA*  T) 

PM  (3,  3)  =-  SASQ 
ELSE 

DO  201  J  =  1,  3 
CALL  RDLINE 

CALL  GETDFP ( 1 . PM (  J. 1 ) . 0 ) 

CALL  GETDFP (2, PM< J-  2),  O) 

CALL  GETDFP (3, PM(J, 3).  O) 

END  IF 

PHI  (1,1)  =  1. 

PHI  (1,2)  =  T 

PHI (1,3)  =  (-1.  +  AT  +  ET)  /  (  ALPHA  *  ALPHA  ) 

PHI  (2,  1  )  =  0. 

PHI  (2,2)  =  1. 

PHI  (2,3)  =  (1.  -  ET)  /  ALPHA 

PHI  (3,  1  )  =  0. 

PHI  (3,  2)  =  O. 

PHI (3, 3)  =  ET 

QQ1  =  1.-  EET  +2.  *AT  +  2.  *AT**3.  /3.  -2.  *AT*AT  -4.  *AT*ET 
QQ2  =  EET  +  1.  -  2.  *ET  +2.  *AT  *ET  -2.  *AT  +  AT*AT 

0(1.1)  =  QQ1  *  SASQ  /  ALPHA**4. 

0(1,2)  =  QQ2  *  SASQ  /  ALPHA**3. 

0(1,3)  =  (1.  -  EET  -2.  *AT*ET)  *  SASQ  /ALPHA**2 

0(2,  1  )  =  Q(  1. 2) 

Q(2, 2)  =  (4.  *ET  -  3.  -EET  +2  *AT ) *  SASQ  /ALPHA**2. 

Q ( 2, 3 )  =  (EET  +  1.  -2.  *ET )  *  SASQ  /  ALPHA 

0(3,  1  )  =  Q(  1,  3) 

Q ( 3,  2)  =  Q ( 2,  3) 

Q ( 3, 3)  =  ( 1  -  EET)  *  SASQ 

IF( ISTATE.  EQ.  2)  THEN 
Q ( 1 ,  1 )  =  SASQ  *  T *T*T  /3. 

Q( 1, 2)  =  SASQ  *  T*T  /2. 

Q(2, 1)  =  Q ( 1 , 2 ) 

0(2, 2)  =  SASQ  *  T 
END  IF 

GENERATE  FICTIOUS  MEASURED  POSITION  DATA  ASSUMING  A  STOCHAST 
MODEL  WITH  ACCLN.  CORRELATION  BETA  AND  ACCLN  VARIANCE  SIAM** 
GENERATE  STM  =  STATE  TRANSITION  MATRIX  TO  BE  USED  TO  CREATE 
THE  FICTIOUS  TARGET  MEASUREMENT  DATA 

AT  =  BETA  *  T 
ET  =  DEXP(-AT) 

EET  =  DEXP ( -2.  *  AT) 

SASQ  =  SIAM  *  SIAM 

STM ( 1,1)  =  1 
STM  <  1,2)  =  T 

STM (1,3)  =  (-1  +  AT  +  ET)  /  (  BETA  *  BETA  ) 

STM  (  2,  1  )  =  0. 

STM  ( 2,  2)  =  1. 

STM (2,3)  =  <1  -  ET)  /  BETA 

STM  ( 3.  1  )  =  0.  A- 5 


M  u 


c 


QQM 1  =  1-  EET  +2.  *AT  +2.  *AT**3.  /3.  -2.  *AT*AT  -4  *AT*ET 

QGM2  =  EET  +  1.  -  2.  *ET  +2.  *AT  *ET  -2.  *AT  +  AT*AT 


C 

C 


C 


C 


C 


C 


GFICT  =  (1.  -  EET)  *  SASQ 
IX  =  I SEED 

CALL  GRAND ( IX, IY, YFL ) 

IX  =  IY 

YFLERR  =  2.  *  (  YFL  -  0.  5  )  *DSGRT<3.  *  R  ) 

S(  1  )  =  0. 

S  <2  )  =  0. 

S ( 3 )  =  A INRAND 
XOLD  =  X INL 
VOLD  =  VINL 

XNEW  =  XOLD  +  T*VOLD  +  0.  5*T*T*AMEAN1 
VNEW  =  VOLD  +  T*AMEAN1 
XTRUE  =  XNEW  +  S( 1 ) 

VTRUE  =  VNEW  +  S<2) 

ATRUE  =  S  ( 3  >  +  AMEAN1 
ZM(1)  =  YFLERR  +  XTRUE 

CALL  GRAND< IX, IY. YFL) 

IX  =  IY 

YFL3  =  2.  *  (  YFL  -  0.  5  )  *DSQRT(3.  *  QFICT  ) 

CALL  GRAND< IX, IY, YFL) 

IX  =  IY 

YFLERR  =  2.  *  <  YFL  -  0.  5  )  *DSQRT<3.  *  R  ) 

SS<1)  =  STM (1<1) *S ( 1 )  STM < 1 >  2  )  *S ( 2 )  +  STM < 1 , 3 ) «S ( 3 ) 
SS  ( 2  )  =  STM<2,  1  )*S<  1  )  STMC2,  2 ) *S ( 2 )  +  STM  (  2,  3 )  *S  (  3  ) 

S3 ( 3 )  =  STM<3.  1)*SC1>  +  STM ( 3,  2 > *S ( 2 )  +  STM ( 3, 3 ) *S ( 3 ) 

DO  33  1  =  1-  3 
S<  I  >  =  SS( I ) 

XOLD  =  XNEW 
VOLD  =  VNEW 

XNEW  =  XOLD  +  T«VOLD  +  O.  5*T*T*AMEAN1 
VNEW  =  VOLD  +  T*AMEAN1 
XTRUE  =  XNEW  +  SCI) 

VTRUE  =  VNEW  +  S<2> 

ATRUE  =  S ( 3 )  +  AMEAN1 

ZM(2)  =  YFLERR  +  XTRUE 

END  OF  FIRST  TWO  MEASUREMENTS  AT  TIME  =  0  AND  TIME  = 
XM  < 1 )  =  ZM<2> 

X  M  (  2  )  =  <  Z M  <  2 )  -  ZM( 1 )  ) /  T 
XM ( 3 )  =  ASTART 

ALTERNATIVELY  INITIALIZED  FILTER  (INITIAL  .  NE  Oil 


IF  (  INITIAL  ME.  0  )  THEN 
XM ( 1 )  =  XST  ART 
X i'l  <  2  )  =  VSTART 
END  IF 

TIME  =  2.  *  T 

A-6 

I COUNT  =  O 
SUM1  =  0. 

SUMP..  =..0 _ _ _ 


YFL3 


non  o  o  o 


SUM3  =  0. 

SUM4  =  0. 

SUM5  =  0. 

GOLD  =  0. 

Z  =  ZMC2) 

C 

IF ( ISTATE.  EG.  2)  THEN 
WRITEC3, 301 ) 

301  FORMAT  (3X.  'TIME',6X,  'MERR  ',  7X,  'PERR',7X,  'VERR'.SX,  'Kl', 

1  10X.  'K2',9X,  'ROOT  P11',5X.  'ROOT  P22  '  ) 

ELSE 

WR I TE ( 3< 302) 

302  FORMAT (3X,  'TIME'.6X,  'MERR'.7X,  'PERR',7X,  'VERR',7X,  'AERR 

1  10X,  'Kl  '.  11X,  'K2  ' »  10X,  'K3  ' .  9X,  'ROOT  Pll  ',  4X,  'ROOT  P22', 

2  'ROOT  P33 ' ) 

END  IF 

C 

C  BEGIN  KALMAN  FILTER  LOOP 

C 

10  CONTINUE 

C 

DEL  =  PM( 1, 1 )  +  R 
GK1  =  PMC  1,  1  >  /  DEL 
GK2  =  PM C 1 ,  2 )  /  DEL 
GK3  =  PM< 1.  3)  /  DEL 
C 

PINV  =  Z  -  XM< 1 ) 

X  < 1 )  =  XM ( 1 )  +  GK1  *  PINV 

X ( 2 )  =  XM C  2 )  +  GK2  *  PINV 

X  C  3 )  =  XM<  3 )  +  GK3  *  PINV 

C 

P  <  1,  1  )  =  (1.  -  GK1  )  *  PM(  1,  1  ) 

P<  1,  2)  =  <  1.  -  GK1  )  *  PM ( 1  ,  2) 

P<  1, 3)  =  (  1.  -  GK1  )  *  PMC  1,  3) 

P(2,  2)  =  -  GK2  *  PMC  1,2)  +  PMC  2,  2) 

PC  2,  3)  =  -  GK2  *  PMC  1,3)  +  PMC  2,  3) 

PC  3,  3)  =  -  GK3  *  PMC  1,3)  +  PMC  3,  3) 

COMPUTE  FADINC  MEMORY  AVERAGE  OF  INNOVATION  PROCESS 

R HO  =  PINV  *  PINV  /  C  PM C 1 ,  1)  +  R  ) 

GNEW  =  GAMMA  *  GOLD  +  RHO 
GOLD  =  GNEW 

WRITE  QUANTITIES  OF  INTEREST 

PERR  =  -X  Cl)  +  XTRUE 

VERR  =  -XC2)  +  VTRUE 

AERR  =  -X ( 3 )  +  ATRUE 

SUM 1  =  SUM 1  +  PERR  *  PERR 
SUM2  =  SUM2  +  YFLERR  *  YFLERR 
SUM3  =  SUM3  +  PERR 
SUM 4  =  SUM 4  YFLERR 
SUM  5  =  SUM  5  +  VERR---VERR 
SPE  =  D5QRTCPMC 1, 1 ) ) 

SPF  =  DSQRTCP  C 1,1)) 

SVE  =  DSQRT (PMC  2, 2) ) 

SVF  =  DSQRT  <  P  C  2, 2) ) 

SAE  =  DSQRT C PM (3, 3>  > 

SAF  =  DSQRTCP  C3,  3) ) 

I COUNT  =  I COUNT  +  1 
C 

IFC  ISTATE  EO.  2  )  THEN  A 

WRITEC3,  100 ) T I ME. YFLERR,  PERR,  VERR, GK1, GK2, SPF, SVF 
FORMAT (F7.  2,  3F1 1.  3.  2X,  4 (Ell.  4,  2X  )  ) 


100 


101 

c 

c 

c 

c 

c 

c 


44 

C 


C 


C 

c 

c 


c 


FORMAT  <F7.  2,  3F1 1  3.  2X.  7 (El  1 .  4,  2X  )  ) 

END  IF 

COMPUTE  PM  AND  XM  FOR  THE  NEXT  TIME  STEP 
IF(GNEW.  GT.  GMAX)  THEN 

ADAPTIVE  FILTER  ....  BEGIN  ADAPTATION 

DO  44  1  =  1, 3 
DO  44  J=l.  3 

QG ( I , J)  =  QFACTOR  *  Q(I,U) 

IF<  ISTATE.  EG.  2  )  THEN 

PM (1.1)  =  P(l.l)+  2.  *T #P ( 1  /  2 ) +  T*T*P<2. 2)  +  QQ(X,I) 

PM(  1,2)  =  P  <  1 ,  2 )  +  T*P(2,2>  +  GQ  (1.2) 

PM  ( 2<  2 )  =  P  (  2i  2)  +  GQ  ( 2/  2 ) 

ELSE 

PM(  1.1)=  P<1,1>+  2.  *T*P ( 1 >  2  )  +  2.  *PHI(1.3)*P(1.3)+  T*T*P(2,2)+ 
1  2  *T*PHI(l,3)#P(2i3)+  PHI  (1,3)  *PHI  (1,3)*P(3»3)+  GQ  (  1 ,  1  ) 

PM  (  1,2)=  P  <  1 ,  2  )  +  PHI (2,  3)*P( 1. 3)  +  T*P<2.2)  +  (PHI(1,3)  + 

1  T#PHI (2, 3) )*P<2, 3)+  PHI(1, 3>*PHI<2, 3>*P<3,  3)+  GQ(1,2) 

PM(1,3)=  PHI (3, 3>*<P< 1, 3)+  T*P(2,3)+  PHI ( 1 , 3 ) *P ( 3,  3 ) ) +GQ ( 1 ,  3 5 
PM  <  2<  2 )  -  P  <  2i  2  )  +  2.  *PHI  (2,  3)  *P  <2.  3>+  PHI  ( 2,  3  )  *PHI  ( 2,  3)  *P  (  3,  3  ) 
1  +  GQ<2, 2) 

PM (2,  3 ) =  PHI (3, 3>*<P<2.  3)  +  PHI (2, 3)*P (3, 3) >+  QQ<2,3) 

PM ( 3,  3 )  =  PHI(3,  3)*PHI<3,  3)*P(3,  3)+  GG(3,3) 

END  IF 
ELSE 

IF (  ISTATE.  EG.  2)  THEN 

PM (1.1)  =  P(l,l>+  2.  *T*P<1,2>+  T*T*P<2,  2)  +  Q(l,l) 

PM  <1.2)  =  P  (  1 1  2  )  +  T*P(2,2>  +  G  (  1 , 2  ) 

PM(2, 2)  =  P(2, 2>+  G(2, 2) 

ELSE 

PM(1.1)=  P<1,1>+  2.  *T#P ( 1 >  2 ) +  2.  *PHI < 1 »  3 ) #P ( 1 »  3 )  +  T* T*P < 2, 2 > + 
1  2.  *T«PHI  <  1, 3)*P<2.  3)+  PHK1,  3)*PHI(1, 3)*P(3,  3)+  Q(l-l) 

PM ( 1 1  2 ) =  P ( 1 , 2)+  PHI(2.  3)*P( 1. 3)+  T*P(2,2)  +  (PHI(1,3;  + 

1  T*PHI <2, 3) )*P<2,  3>  +  PHI ( 1, 3)*PHI (2, 3)*P(3. 3)+  Q(l,2> 

PM(1,3)=  PHI  (3,  3)*(P(  1, 3)+  T  *P  (2.  3  )  +  PHI  (  1 , 3  )  *P  (  3,  3  )  )  +  Q(l,3) 
PM  <  2>  2  )  =  P<2,2>  +  2.  *PHI  (2,  3)*P(2,  3)+  PHI  <  2,  3  )  *PHI  <  2,  3  )  *P  (  3,  3  ) 
1  +  Q  <  2,  2  ) 

PM  (2.  3 )  =  PHI  <3.  3>*<P<2,  3)+  PHI  (2.  3  )  *P  (  3.  3)  )  +  Q<2,3) 

PM  (  3<  3  )  =  PHI  (3,  3>*PHI  (3,  3  >  *P  (3.  3)  +  G(3<3) 

END  IF 
END  IF 

END  OF  ADAPTION . 

I F (  ISTATE  EG.  2  )  THEN 
XM< 1 )  =  X  < 1 )  +  T*X ( 2 ) 

X  M  ( 2 )  =  X  <  2 ) 

ELSE 

X  M  <  1  !  =  X  <  1  )  +  T-*X(2>  +  PHI (  1. 3)  *X(3J 
X  it  <  2 )  =  X  ( 2  )  +  PHI (2. 3) *X (3) 

XM(3)  =  PHI <3,  3>*X(3) 

END  IF 

TIME  =  TIME  +  T 
IF(TIME.  GT.  2.  >  R  =--  AA*AA 
IF (TIME. GT.  4.  )  R  =  BB*BB 
IF ( TIME. GT  6.  )  R  =  CC*CC 
IF(TIME.  GT.  8.  )  R  =  DD*DD  A-8 
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n  n  o 


C 


C 

34 

C 


C 


C 


C 
1  1 


103 

104 

105 


CALL  GRAND< IX. IY, YFL) 

IX  =  IY 

YFL3  =  2. *  (  YFL  -05)  *DSQRT(3.  *  QFICT  ) 

CALL  GRAND! IX. IY, YFL) 

IX  =  IY 

YFLERR  =  2. *  (  YFL  -  0.  5  )  *DSQRT(3.  *  R  ) 

SS(1)  =  STM  < 1 ,  1 ) *S ( 1 )  +  STM (1,2) *S (2 )  +  STM(1,3)*S( 

SS ( 2 )  =  STM (2,1) *S  < 1 )  +  STM(2. 2 >*S(2)  +  STM(2.3)*S( 

SS ( 3  )  =  STM (3,1) *S ( 1 )  +  STM (3,2) *S ( 2 )  +  STM (3, 3)*S( 

DO  34  1  =  1,  3 
S ( I )  =  SS  <  I  ) 

IF  (TIME.  LT.  Tl.  OR.  TIME.  GT.  T2)  THEN 
AMEAN  =  AMEAN1 

ELSE 

AMEAN  =  AMEAN2 
END  IF 

XOLD  =  XNEW 

VOLD  =  VNEW 

XNEW  =  XOLD  +  T*VOLD  +  0. 5*T*T*AMEAN 

VNEW  =  VOLD  +  T* AMEAN 

XTRUE  =  XNEW  +  S<1) 

VTRUE  =  VNEW  +  S(2) 

A  TRUE  =  S (3)  +  AMEAN 
Z  =  YFLERR  +  XTRUE 

I F ( T I ME .  GT.  TMAX )  GO  TD  11 
GO  TO  10 

SMEAS  =  DSQRT ( SUM2/ I COUNT ) 

SFPOS  =  DSQRT ( SUM1 / ICOUNT ) 

SUM3  =  SUN3  /  ICOUNT 

SUM4  =  SUM4  /  ICOUNT 

SUM5  =  DSQRT (SUM5/ ICOUNT) 

WRITE (3, 1031SUM4, SMEAS 
FORMAT!/.  'MEAN  MEAS  ERR  =',F10  4,  '  RMS  MEAS  ERR  =',F10.  4) 
WRITE (3, 1 04 ) SUM3, SFPOS 

FORMAT!/,  'MEAN  PESTM  ERR  =',F10.  4,  '  RMS  PESTM  ERR  =',F10.  4) 
WR ITE ( 3, 105 ) SUM5 

FORMAT  (/,  'RMS  VESTM  ERR  =',F10.  4) 

STOP 

END 


RANDOM  NUMBER  GENERATOR  SUBROUTINE 


SUBROUTINE  GRAND ( I X,  I Y, YFL ) 
IY  =  IX  *  65539 
IF  ( I Y )  5,  6,  6 

5  IY  =  IY  +  2147483647  +  1 

6  YFL  =  IY 

YFL  =  YFL  *  0  46566 13E -9 

RETURN 

END 
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SAMPLE  INPUT  FILE  FOR  KALMAN  FILTER  PROGRAM 
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F,  ALPHA,  SI  A,  SIN,  TMAX 

SIAM,  BETA,  XINL,  VINL,  AINRAND 

AMEAN1, Tl, T2, AMEAN2 

XSTART, VSTART, ASTART 

ISEFD.  INITIAL,  ISTATE 

GAMMA, GMAX, QF ACTOR 

AA,  BB, CC,  DD 

PM  (  1 ,  1  >,  PM(  1, 2),  PM<  1, 3) 

PM  (2,  1  > ,  PM  (2,  2  ) ,  PM  (2,  3) 

PM  (  3,  1),  P  MO,  2),  PM  (  3,  3> 
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